#include <stdbool.h>
#include <ucos_ii.h>
#include <board.h>
#include <bsp.h>
#include <osutils.h>
#include <leds.h>
#include <buttons.h>
#include <lcd.h>
#include <potentiometer.h>
#include <accelerometer.h>
#include <interface.h>
#include <control.h>
#include <buffer.h>
#include <robot_movements.h>
#include <messages.h>
#include <string.h>
#include <stdint.h>
#include <global_decs.h>

/* Define at most one of CONTROL_TEST, CONVEYOR_TEST or ROBOT_TEST */
#define ROBOT_TEST 1

#define BUTTONS_TASK_ID 0

/*
*********************************************************************************************************
*                                            PRIORITIES
*********************************************************************************************************
*/

#define  APP_TASK_EMERGENCY_PRIO                   4
#define  APP_TASK_READ_CAN_PRIO                    5
#define  APP_TASK_MOVE_ARMS_PRIO                   10
#define  APP_TASK_BUTTONS_PRIO                     12
#define  APP_TASK_DISPLAY_PRIO                     15

/*
*********************************************************************************************************
*                                            APPLICATION TASK STACKS
*********************************************************************************************************
*/

#define  APP_TASK_CAN_STK_SIZE                256
#define  APP_TASK_MOVE_ARMS_STK_SIZE          256
#define  APP_TASK_READ_CAN_STK_SIZE           256
#define  APP_TASK_BUTTONS_STK_SIZE            256
#define  APP_TASK_DISPLAY_STK_SIZE            256
#define  APP_TASK_EMERGENCY_STK_SIZE          256

static OS_STK appTaskMoveArmsStk[APP_TASK_MOVE_ARMS_STK_SIZE];
static OS_STK appTaskReadCanStk[APP_TASK_READ_CAN_STK_SIZE];
static OS_STK appTaskButtonsStk[APP_TASK_BUTTONS_STK_SIZE];
static OS_STK appTaskDisplayStk[APP_TASK_DISPLAY_STK_SIZE];
static OS_STK appTaskEmergencyStk[APP_TASK_CAN_STK_SIZE];

/*
*********************************************************************************************************
*                                            APPLICATION FUNCTION PROTOTYPES
*********************************************************************************************************
*/

static void appTaskMoveArms(void *pdata);
static void appTaskReadCan(void *pdata);
static void appTaskButtons(void *pdata);
static void appTaskDisplay(void *pdata);
static void appTaskEmergencyStop(void *pdata);
static void initEmergency(void);
void startSystem(bool);
/*
*********************************************************************************************************
*                                          OTHER LOCAL FUNCTION PROTOTYPES
*********************************************************************************************************
*/

/*
*********************************************************************************************************
*                                            GLOBAL VARIABLE DEFINITIONS
*********************************************************************************************************
*/
bool armPermitted = false;
bool armPermittedDeposit = false;

messageData_t received_message;
messageData_t temp_message;
 

              int main() {

  /* Initialise the OS */
  OSInit();                                                   

  startSystem(0);
  
  /* Start the OS */
  OSStart();
    
  /* Should never arrive here */ 
  return 0;      
}

/*
*********************************************************************************************************
*                                            APPLICATION TASK DEFINITIONS
*********************************************************************************************************
*
*/

static void appTaskMoveArms(void *pdata) {
    
   bool conOutPadStateFull = false;
   bool outPadStateFull = false;
  
   while(true) {
      
      conOutPadStateFull = getPadState(CON_OUT_PAD);
      outPadStateFull = getPadState(OUT_PAD);
      
      //If the robot state is idle continue with action
      if(getRobotState() == IDLE && getState() != PAUSE){
        
        //Only proceed if the conveyor has item waiting
        if(conOutPadStateFull){
          
          //If the control panel is signalling a no go keep asking until it's free
          if(outPadStateFull){
            
            //Ask control if its free for a block to be deposited
            messageData_t temp_message = {ID_CONTROLP,MY_ID,MSG_PAD_SENS2_STAT_REQ};
            writeMsgToCan(&temp_message);
            
            OSTimeDly(400);
            continue; //reset loop
          }
          
          setRobotState(MOVING);
          //Attempt block pickup 3 times
          Default_Positions();
          for(int i = 0; i<3; ++i){
            
            //if the conveyor is full, it's either the 
            //first try or last attempt failed
            if(getPadState(CON_OUT_PAD)){
              Robot_Remove_Block_Part1();
              
              messageData_t temp_message = {ID_CONVEYOR,MY_ID,MSG_PICKUP_DEPOSIT_DONE};
              writeMsgToCan(&temp_message);
              OSTimeDly(200);
            }
            else{
              break;  //break out if successful
            }
          }
          //If still in this state 3 attempts failed, throw emergency
          if(getPadState(CON_OUT_PAD)){
            messageData_t temp_message = {ID_CONTROLP,MY_ID, MSG_EMERG_STOP};
            writeMsgToCan(&temp_message);
          }
          
          //If here pickup was sucessful, attempt deposit 3 times
          for(int i = 0; i<3; ++i){
            
            //If empty it was first attempt or last attempt failed
            if(!getPadState(OUT_PAD)){
              
              Robot_Remove_Block_Part2();
              
              messageData_t temp_message = {ID_CONTROLP,MY_ID,MSG_PICKUP_DEPOSIT_DONE};
              writeMsgToCan(&temp_message);
              OSTimeDly(200);
            }
            else{
              break;  //Reaching here means success, break out
            }
          }
          //If this is true deposit was unsuccessful, throw emergency
          if(!getPadState(OUT_PAD)){
            messageData_t temp_message = {ID_CONTROLP,MY_ID, MSG_EMERG_STOP};
            writeMsgToCan(&temp_message);
          }
          
          setRobotState(IDLE);      
        }//inpad full
      }//inpad idle
      
      
      OSTimeDly(500);
    }
  
}

/*************************************************
 * Function : appTaskButtons
 * Desc     : Polls the buttons for simulations
 * Return   : Void
 * Params   : Void
 *
 *^z**********************************************/
static void appTaskButtons(void *pdata) {
    
  while(true){
    
    //SIMULATE EMERGENCY
    if (isButtonPressed(JS_LEFT)) {
      initEmergency();   
    }
    //SIMULATE PAUSE
    if (isButtonPressed(JS_UP)) {
      setState(PAUSE);   
    }
    
    //SIMULATE STOP
    else if (isButtonPressed(JS_RIGHT)) {
      setState(STOP);   
    }
    
    //SIMULATE RESUME
    else if (isButtonPressed(JS_DOWN)) {
      setState(RUNNING);   
    }
    
    //SIMULATE START
    else if (isButtonPressed(BUT_1)) {
      Robot_Remove_Block_Part1();
      Robot_Remove_Block_Part2();
    }
       
   OSTimeDly(20);
  }
}

static void appTaskDisplay(void *pdata){
   
  while(true){
     messageData_t last_message;
     getLastMsg(&last_message,MY_ID);
    
     lcdSetTextPos(2,2);
     lcdWrite("READ  :    WRITE");
     lcdSetTextPos(2,3);
     lcdWrite("T.ID : %d - %d", received_message.targetID, last_message.targetID);
     lcdSetTextPos(2,4); 
     lcdWrite("O.ID : %d - %d", received_message.originatorID, last_message.originatorID);     
     lcdSetTextPos(2,5);
     lcdWrite("READ : %d - %d", received_message.content, last_message.content);
     
     lcdWrite("Hand:%0d", robotJointGetState(ROBOT_HAND));
     lcdSetTextPos(2, 7);
     lcdWrite("Wrist:%0d", robotJointGetState(ROBOT_WRIST));
     lcdSetTextPos(2, 8);
     lcdWrite("Elbow:%0d", robotJointGetState(ROBOT_ELBOW));     
     lcdSetTextPos(2, 9);
     lcdWrite("Waist:%0d", robotJointGetState(ROBOT_WAIST));  
     OSTimeDly(500);
  }
}


static void appTaskReadCan(void *pdata) {
  
  bspInit();
  robotInit();
  osStartTick();
  
    messageData_t temp_message = {ID_CONTROLP,MY_ID,MSG_READY};
            writeMsgToCan(&temp_message);
    
  while(true){
  
  if(readMsgFromCan(&received_message)) {
    
       //Create message structure
        messageData_t lastWrite;
        getLastMsg(&lastWrite, MY_ID);
        
        //Control panel showing inpad is full
        if((received_message.content == MSG_SENS1_FULL)) {
          if (received_message.originatorID == ID_CONVEYOR) {
            
            setPadState(CON_OUT_PAD, true);
            sendACK(ID_CONVEYOR);
            
          }
        }
        
        //Control panel showing inpad is empty
        if((received_message.content == MSG_SENS1_EMPTY)) {
          if (received_message.originatorID == ID_CONVEYOR) {
            
            setPadState(CON_OUT_PAD, false);
            //sendACK(ID_CONTROLP);
            
          }
        }
        
        //Message received saying conveyor is free
        else if((received_message.content == MSG_SENS2_EMPTY) &&
          (received_message.originatorID == ID_CONTROLP)) {
          
            setPadState(OUT_PAD, false);
            //sendACK(IN_CONTROLP);
        }
        
        //Message received saying conveyor is full
        if((received_message.content == MSG_SENS2_FULL)) {
          if (received_message.originatorID == ID_CONTROLP) {
            
            setPadState(OUT_PAD, true);
            //sendACK(IN_CONTROLP);
          }
        }
        
        else if(received_message.content == MSG_EMERG_STOP) 
        {
          initEmergency();
        }
        
        else if(received_message.content == MSG_PAUSE) 
        {
          setState(PAUSE);
          sendACK(ID_CONTROLP);
        }
        
        else if(received_message.content == MSG_RESUME) 
        {
          setState(RUNNING);
          sendACK(ID_CONTROLP);
        }
    } 
    OSTimeDly(10);  
  }
}

/*************************************************
 * Function : initEmergency
 * Desc     : Initiate task to handle emergency stop.
 * Return   : Void
 * Params   : Void
 *^z**********************************************/
void initEmergency(void){
  
  OSTaskCreate(appTaskEmergencyStop,                               
               (void *)0,
               (OS_STK *)&appTaskEmergencyStk[APP_TASK_EMERGENCY_STK_SIZE - 1],
               APP_TASK_EMERGENCY_PRIO);
}

/*************************************************
 * Function : appTaskEmergencyStop
 * Desc     : Kill system and notify user vua GUI.
 * Return   : Void
 * Params   : Void
 *
 * TODO     : Reset on button press?
 *^z**********************************************/
void appTaskEmergencyStop(void *pdata){
  
  //ACK CONTROL PANEL FOR EMERGENCY
  sendACK(ID_CONTROLP);
   
  //Kill all the running tasks to prevent further movement
  OSTaskDel(APP_TASK_READ_CAN_PRIO);
  OSTaskDel(APP_TASK_MOVE_ARMS_PRIO); 
  OSTaskDel(APP_TASK_DISPLAY_PRIO);

   //notify user of emergency situation
  printf( "\f" );
  lcdSetTextPos(2, 5);
  lcdWrite("SHUTDOWN");
  lcdSetTextPos(2, 10);
  lcdWrite("HOLD BUT2 FOR RESET");
  
  ledSetState(USB_LINK_LED, LED_OFF);
  ledSetState(USB_CONNECT_LED, LED_ON);
  
  //flag for screen flash
  bool flash = false;
  
  while(true){
    
    if (isButtonPressed(BUT_2)) {
      
      printf( "\f" );
      ledSetState(USB_LINK_LED, LED_OFF);
      ledSetState(USB_CONNECT_LED, LED_OFF);
      
      startSystem(1);
      OSTaskDel(OS_PRIO_SELF);
      break;
    }
    
    //flash screen
    lcdSetTextPos(2, 7);
    if(flash){
      lcdWrite("                    ");
      flash = false;
    }
    else{
      
      lcdWrite("EMERGENCY STOP!");
      flash = true;
    }
    
    //flash LEDs
    ledToggle(USB_LINK_LED);
    ledToggle(USB_CONNECT_LED);
           
    OSTimeDly(1000);
  }
  
}

/*************************************************
 * Function : startSystem
 * Desc     : Initialise system and create system tasks
 * Return   : Void
 * Params   : bool
 *^z**********************************************/
void startSystem(bool started){
  
  //First time initialise board/robot/tick
  if(!started){
    bspInit();
    robotInit();
    osStartTick();
  }
  
  //Initialise arm flags to empty system
  armPermitted = false;
  armPermittedDeposit = false;
  initRobotVars();
  
/* Create Tasks */
  
  OSTaskCreate(appTaskMoveArms,                               
               (void *)0,
               (OS_STK *)&appTaskMoveArmsStk[APP_TASK_MOVE_ARMS_STK_SIZE - 1],
               APP_TASK_MOVE_ARMS_PRIO);
  
  OSTaskCreate(appTaskReadCan,                               
               (void *)0,
               (OS_STK *)&appTaskReadCanStk[APP_TASK_READ_CAN_STK_SIZE - 1],
               APP_TASK_READ_CAN_PRIO);
  
   OSTaskCreate(appTaskButtons,                               
               (void *)0,
               (OS_STK *)&appTaskButtonsStk[APP_TASK_BUTTONS_STK_SIZE - 1],
               APP_TASK_BUTTONS_PRIO);
    
  OSTaskCreate(appTaskDisplay,                               
               (void *)0,
               (OS_STK *)&appTaskDisplayStk[APP_TASK_DISPLAY_STK_SIZE - 1],
               APP_TASK_DISPLAY_PRIO);
  
}